/*
 * SensorService.java
 *
 * Created on June 1, 2008, 9:52 AM
 *
 * This daemon thread pulls sensor readings from the various external sensors
 * on the robot, and then pushes them to the Actors via Actor.updateSensors()
 * SensorService runs locally on the SPOT with a faster update interval than
 * Mainloop, allowing for more sensor readings before an action is made.
 * Note that intrinsic sensors are not managed here.
 */

package org.gunncs.daemons;

import java.io.*;
import org.gunncs.actoriface.Robot;
import com.iaroc.irobot.*;
import com.sun.spot.sensorboard.io.*;
import java.util.*;
import org.gunncs.actoriface.*;
import com.sun.spot.sensorboard.EDemoBoard;
import com.sun.spot.sensorboard.peripheral.LEDColor;
import com.sun.spot.sensorboard.peripheral.ITriColorLED;
/**
 *
 * @author Anandg, ajc
 * 
 */
public class SensorService extends Thread{
    public static final int POLLTIME = 50;  //Refresh rate for sensor polling
    
    //Indicies of robot sensors to be polled
	  public static int[] sensorIndex = {1,2,3,4,5,7};
	  
	  
    Robot bot;
    Vector actors;
    boolean running;
    
    ITriColorLED overallStatus = EDemoBoard.getInstance().getLEDs()[2];
    
    /** Creates a new instance of SensorService */
    public SensorService(Robot r) {
        bot=r;
        actors = new Vector();
    }
    
    public void init(){
        Vector mpactors = Mainloop.getInstance().actors;
        for(int i=0; i<mpactors.size(); i++) {
            System.out.println("adding " +((Actor) (mpactors.elementAt(i))));
            addActor((Actor) mpactors.elementAt(i));
        }
    }

    public void addActor(Actor a){
        this.actors.addElement(a);
    }

    public void run(){
      //LED status GREEN = SENSORS DATA ACQUIRED
      overallStatus.setColor(LEDColor.GREEN);
      overallStatus.setOn();
       running = true;
       System.out.println("Starting Sensor Service");
       try{                     
        while(running){
            this.sleep(POLLTIME);
            //LED status BLUE = READY
            overallStatus.setColor(LEDColor.BLUE);
            for(int j=0; j<sensorIndex.length; j++){
                  try{
                      bot.getSensors()[sensorIndex[j]].update(this);
                  }catch(IOException e){
                        e.printStackTrace();
                        //LED status ORANGE = EXCEPTION IN POLLING
                        overallStatus.setColor(LEDColor.ORANGE);
                  }       
            }
            //LED status GREEN = POLLED
            overallStatus.setColor(LEDColor.GREEN);
            for(int i=0; i<actors.size(); i++){
              ((Actor) (actors.elementAt(i)) ).updateSensors(bot.getSensors());
            }
        }
       }catch(Exception e){
           e.printStackTrace();
       }
       
       //LED status RED = FATAL EXCEPTION
       overallStatus.setColor(LEDColor.RED);
       System.out.println("Sensor Thread dying");
    }
}
